import rclpy
from rclpy.node import Node
from face_detect_info.srv import FaceInfo
import cv2
from cv_bridge import CvBridge
import face_recognition
import time


class FaceDetectServer(Node):
    def __init__(self, name):
        super().__init__(node_name=name)
        self._service = self.create_service(
            FaceInfo, "/face_dect", self._service_callback
        )
        self._upsample_times = 1
        self._model = "hog"
        self._cv_brige = CvBridge()

        self.get_logger().info(
            f"face detect service start, time:{self.time_translate(time.time())}"
        )

    def _service_callback(self, request, response):
        if request.image.data:
            cv_image = self._cv_brige.imgmsg_to_cv2(request.image)
        else:
            response.invalid = True
            return response

        start_time = time.time()
        self.get_logger().info(
            f"load image, start time:{self.time_translate(start_time)}"
        )
        face_locations = face_recognition.face_locations(
            cv_image,
            number_of_times_to_upsample=self._upsample_times,
            model=self._model,
        )
        end_time = time.time()
        self.get_logger().info(f"load image, end time:{self.time_translate(end_time)}")

        response.invalid = False
        response.number = len(face_locations)
        response.use_time = end_time - start_time
        for top, right, bottom, left in face_locations:
            response.top.append(top)
            response.right.append(right)
            response.bottom.append(bottom)
            response.left.append(left)
        self._show_face_location(response, cv_image)
        return response

    def time_translate(self, time_in):
        time_struct = time.localtime(time_in)
        time_standard = time.strftime("%Y-%m-%d %H:%M:%S", time_struct)
        return time_standard


    def _show_face_location(self, response, img):
        # for i in range(response.number):
        #     top = response.top[i]
        #     right = response.right[i]
        #     bottom = response.bottom[i]
        #     left = response.left[i]
        #     cv2.rectangle(img, (left, top), (right, bottom), (0, 0, 255), 2)
        #     self.get_logger().info(f"image center:{(top + bottom)/2},{(left+right)/2}")

        cv2.imshow("face rec", img)
        cv2.waitKey(20)



def main():
    rclpy.init()
    node = FaceDetectServer("face_detect")
    rclpy.spin(node)
    rclpy.shutdown()
